#include <offect.h>
#include <mm.h>
#include <sysregs.h>
#include <esr.h>

#define BAD_SYNC        0
#define BAD_IRQ         1
#define BAD_FIQ         2
#define BAD_ERROR       3


/* origin sp */
/* S_PC*/
/* ....*/
/* x0  */
/* new sp */

.macro vm_exit el
    sub sp, sp, #S_FRAME_SIZE

    stp x0, x1, [sp, #16 * 0]
    stp x2, x3, [sp, #16 * 1]
    stp x4, x5, [sp, #16 * 2]
    stp x6, x7, [sp, #16 * 3]
    stp x8, x9, [sp, #16 * 4]
    stp x10, x11, [sp, #16 * 5]
    stp x12, x13, [sp, #16 * 6]
    stp x14, x15, [sp, #16 * 7]    
    stp x16, x17, [sp, #16 * 8]
    stp x18, x19, [sp, #16 * 9] 
    stp x20, x21, [sp, #16 * 10]
    stp x22, x23, [sp, #16 * 11]
    stp x24, x25, [sp, #16 * 12]
    stp x26, x27, [sp, #16 * 13]
    stp x28, x29, [sp, #16 * 14]

    .if \el == 1
        mrs x21, sp_el1
    .else
        add x21, sp, #S_FRAME_SIZE
    .endif

    mrs x22, elr_el2
    mrs x23, spsr_el2

    stp lr, x21, [sp, #S_LR]
    stp x22, x23, [sp, #S_PC]
.endm

.macro inv_entry el, reason
    mov x0, sp
    mov x1, #\reason
    mrs x2, esr_el2
    b bad_mode
.endm

.macro vtentry lable
    .align 7
    b \lable
.endm

.macro vm_entry el
    ldp x21, x22, [sp, #S_PC]
    msr elr_el2, x21
    msr spsr_el2, x22
    .if \el == 1
        ldr x23, [sp, #S_SP]
        msr sp_el1, x23
    .endif    
    
    ldp x0, x1, [sp, #16 * 0]
    ldp x2, x3, [sp, #16 * 1]
    ldp x4, x5, [sp, #16 * 2]
    ldp x6, x7, [sp, #16 * 3]
    ldp x8, x9, [sp, #16 * 4]
    ldp x10, x11, [sp, #16 * 5]
    ldp x12, x13, [sp, #16 * 6]
    ldp x14, x15, [sp, #16 * 7]
    ldp x16, x17, [sp, #16 * 8]
    ldp x18, x19, [sp, #16 * 9]
    ldp x20, x21, [sp, #16 * 10]
    ldp x22, x23, [sp, #16 * 11]
    ldp x24, x25, [sp, #16 * 12]
    ldp x26, x27, [sp, #16 * 13]
    ldp x28, x29, [sp, #16 * 14]

    ldr lr, [sp, #S_LR]
    add sp, sp, #S_FRAME_SIZE
    eret
.endm

/* Vector Table*/
.align 11
.global virt_vectors
virt_vectors:

/*use sp_el0 in current el*/
    vtentry el2_sync_invalid
    vtentry el2_irq_invalid
    vtentry el2_fiq_invalid
    vtentry el2_error_invalid
/*use current sp in current el*/
    vtentry el2_sync_invalid
    vtentry el2_irq_invalid
    vtentry el2_fiq_invalid
    vtentry el2_error_invalid

/*EL level change in arch64*/
    vtentry el1_exit
    vtentry el1_irq_invalid
    vtentry el1_fiq_invalid
    vtentry el1_error_invalid

/*EL level change in arch32*/
    vtentry el1_sync_invalid
    vtentry el1_irq_invalid
    vtentry el1_fiq_invalid
    vtentry el1_error_invalid

el1_exit:
    vm_exit 1
    mrs x25, esr_el2
    lsr x24, x25, #ESR_ELx_EC_SHIFT
    cmp x24, #ESR_ELx_EC_HVC64
    b.eq vm_hvc
    cmp x24, #ESR_ELx_EC_DABT_LOW
    b.eq vm_da
    cmp x24, #ESR_ELx_EC_IABT_LOW
    b.eq vm_ia
    bl bad_mode

vm_ia:
vm_da:
    mrs x0, hpfar_el2
    mov x1, x25
    mov x2, sp
    bl do_vm_mem_abort
    vm_entry 1

vm_hvc:
    mov x0, sp
    bl vm_hvc_handler
    vm_entry 1

el2_irq_invalid:
	inv_entry 1, BAD_IRQ
el2_sync_invalid:
	inv_entry 1, BAD_SYNC
el2_fiq_invalid:
	inv_entry 1, BAD_FIQ
el2_error_invalid:
	inv_entry 1, BAD_ERROR
el1_sync_invalid:
	inv_entry 0, BAD_SYNC
el1_irq_invalid:
	inv_entry 0, BAD_IRQ
el1_fiq_invalid:
	inv_entry 0, BAD_FIQ
el1_error_invalid:
	inv_entry 0, BAD_ERROR


.global jump_to_vm
jump_to_vm:
    ldr x24, =HCR_HOST_NVHE_FLAGS
    msr hcr_el2, x24

    /*disable_mmu for el1*/
    ldr x24, =SCTLR_VALUE_MMU_DISABLED
    msr sctlr_el1, x24

    /*set exception return el1 */
    ldr x24, =SPSR_EL1
    msr spsr_el2, x24

    /*vm entry addr */
    adr x24, vm_entry
    msr elr_el2, x24

    /* stack size : 4K */
    adr x2, vm_sp
    add x2, x2, #4096
    msr sp_el1, x2

    eret

vm_entry:
    ret
